#ifndef GMM_VOXEL_MAP_BASE_H
#define GMM_VOXEL_MAP_BASE_H


#include <iostream>
#include <Eigen/Core>
#include <vector>
#include <ros/ros.h>
namespace gvm
{

template <typename _Datatype>	
class VoxelMapCircularBufferBase
{
public:
	VoxelMapCircularBufferBase(){}
	~VoxelMapCircularBufferBase(){}

	VoxelMapCircularBufferBase(double resolution,_Datatype empty_element, int POW)
	: resolution_(resolution), empty_element_(empty_element),_N(1<<POW),_N_2((1<<POW)/2),
	 buffer_((1<<POW) * (1<<POW)  * (1<<POW) ) 
	{
		offset_<<-_N_2,-_N_2,-_N_2;
		std::cout<<"_N: "<<_N<<std::endl;
		std::cout<<"_N_2: "<<_N_2<<std::endl;

		_MASK = (_N - 1);
		std::cout<<"offset: "<<offset_<<std::endl;
		std::cout<<"buffer_ SIZE: "<<buffer_.size()<<std::endl;
		std::fill(buffer_.begin(), buffer_.end(), empty_element_);

		mapCentreIdx<<_N_2,_N_2,_N_2;
		mapCentrePos<<0 ,0 ,0;

		offset<<0,0,0;
		offset_sum<<0,0,0;

		std::cout<<"map range: "<<_N*resolution<<"*"<<_N*resolution<<std::endl;
		nonSenseVariable=0;

	}

	Eigen::Vector3d initPos;

	std::vector<_Datatype> buffer_;
	double resolution_;
	_Datatype empty_element_;
	Eigen::Vector3i offset_;
	int POW_;
	int _N;
	int _N_2;
	int _MASK;

	Eigen::Vector3i mapCentreIdx;
	Eigen::Vector3d mapCentrePos;              // discrete position in global coordinate

	Eigen::Vector3i offset;
	Eigen::Vector3i offset_sum;
	_Datatype nonSenseVariable;


public:

	inline bool insideRange(Eigen::Vector3i idx)
	{
		return (idx(0)<_N && idx(1)<_N && idx(2)<_N && idx(0)>=0 && idx(1)>=0 && idx(2)>=0);
	}

	inline _Datatype getLocalIdxOccupiedProb(Eigen::Vector3i idx)
	{
		return at(idx);
	}

	inline _Datatype getGlobalIdxOccupiedProb(Eigen::Vector3i idx)  // idx is in global map, that is, (0,0,0)'s index is (0,0,0)
	{
		Eigen::Vector3i initGlobalIdx= ((initPos) / resolution_).array().floor().template cast<int>();

		Eigen::Vector3i local_index;
		local_index=idx-initGlobalIdx-offset_sum+mapCentreIdx;
		return at(local_index);
	}

	inline _Datatype getPosOccupiedProb(Eigen::Vector3d pos)  //pos is the global position
	{
		Eigen::Vector3i pos_index;
		getIdxLocal(pos,pos_index);

		return at(pos_index);
	}


	Eigen::Vector3d getGlobalPosfromLocalIdx(Eigen::Vector3i idx)
	{
		Eigen::Vector3d tmp=resolution_*(idx-mapCentreIdx).cast<double> ();
		return tmp+mapCentrePos;
	}

	inline _Datatype & at(const Eigen::Vector3i &index)  // index is range from (0,0,0)~(N-1,N-1,N-1) in local Map
	{
		// if(!insideRange(index))
		// {
		// 	ROS_ERROR("Index: %d %d %d is out of range, can not change it value!!!!!",index(0),index(1),index(2));
		// 	return nonSenseVariable;
		// }
		int buffer_idx=0;

		int buffer_idx_x=((index(0)+offset_sum(0))&_MASK);
		int buffer_idx_y=((index(1)+offset_sum(1))&_MASK);
		int buffer_idx_z=((index(2)+offset_sum(2))&_MASK);

		buffer_idx=buffer_idx_x + _N * buffer_idx_y+_N * _N *  buffer_idx_z; //buffer_idx: 0~(N^3)-1

		return buffer_[buffer_idx];
	}

	inline _Datatype  at(const Eigen::Vector3i &index)  const
	{
		// if(!insideRange(index))
		// {
		// 	ROS_ERROR("Index: %d %d %d is out of map range, can not get it value!!!!!",index(0),index(1),index(2));
		// 	return empty_element_;
		// }

		int buffer_idx=0;

		int buffer_idx_x=((index(0)+offset_sum(0))&_MASK);
		int buffer_idx_y=((index(1)+offset_sum(1))&_MASK);
		int buffer_idx_z=((index(2)+offset_sum(2))&_MASK);

		buffer_idx=buffer_idx_x + _N * buffer_idx_y+_N * _N *  buffer_idx_z;

		return buffer_[buffer_idx];
	}


	//get point index in local map  (0,0,0)~(N-1,N-1,N-1) , point is in global map
	inline void getIdxLocal(const Eigen::Vector3d & point, Eigen::Vector3i & idx) const 
	{ 
		Eigen::Vector3d tmp_point=point;
		tmp_point(0)+=0.001;
		tmp_point(1)+=0.001;
		tmp_point(2)+=0.001;

		idx = ((tmp_point-mapCentrePos) / resolution_).array().floor().template cast<int>();
		idx+=mapCentreIdx;
	}


	//get point index in global map  (-inf,-inf,-inf)~(inf,inf,inf)
	inline void getIdxGlobal(const Eigen::Vector3d & point, Eigen::Vector3i & idx) const 
	{ 
		idx = (point / resolution_).array().floor().template cast<int>();
	}

	Eigen::Vector3i getClosestIdxInRange(Eigen::Vector3d originPos,Eigen::Vector3d endpoint)
	{	
		Eigen::Vector3d diff = (endpoint - mapCentrePos);

		double min_t =9999;

		int direction_index=0;

		int sign=1;

		for (int i = 0; i < 3; i++) 
		{
			if (std::abs(diff(i)) > 0) 
			{
				double ratio=(_N_2)*resolution_/double(fabs(diff(i)));
				// min_t=std::min(min_t,ratio);
				if(ratio<min_t)
				{
					min_t=ratio;
					direction_index=i;
					sign=diff(i)>0?1:-1;
				}
			}
		}
		double xb;
		xb=mapCentrePos(direction_index)+sign*(_N_2-1)*resolution_;


		Eigen::Vector3d closestPos;

		double ratio=(xb-originPos(direction_index))/(endpoint(direction_index)-originPos(direction_index));

		closestPos=originPos+ratio*(endpoint-originPos);
		// ROS_INFO("closestPos: %f %f %f",closestPos(0),closestPos(1),closestPos(2));
		Eigen::Vector3i closestIdx;
		getIdxLocal(closestPos,closestIdx);

		// std::cout<<"min_t: "<<min_t<<std::endl;
		return closestIdx;	
	}


	void updateMapOrigin(Eigen::Vector3d new_originPos)
	{
		// new_originPos(0)+=0.001;
		// new_originPos(1)+=0.001;
		// new_originPos(2)+=0.001;

		// ROS_INFO("enter into updateMapOrigin");
		// ROS_INFO("new_originPos: %f %f %f",new_originPos(0),new_originPos(1),new_originPos(2));
		Eigen::Vector3i new_originIdx_offset;

		Eigen::Vector3d tmp_new_originPos=new_originPos;
		tmp_new_originPos(0)+=1e-6;
		tmp_new_originPos(1)+=1e-6;
		tmp_new_originPos(2)+=1e-6;
		// ROS_INFO("tmp_new_originPos: %f %f %f",tmp_new_originPos(0),tmp_new_originPos(1),tmp_new_originPos(2));

		Eigen::Vector3d tmp=(tmp_new_originPos-mapCentrePos) / resolution_;
		// ROS_INFO("resolution: %f _N: %d",resolution_,_N);
		// ROS_INFO("mapCentrePos: %f %f %f",mapCentrePos(0),mapCentrePos(1),mapCentrePos(2));

		// ROS_INFO("tmp: %f %f %f",tmp(0),tmp(1),tmp(2));

		new_originIdx_offset(0)=floor(tmp(0));
		new_originIdx_offset(1)=floor(tmp(1));
		new_originIdx_offset(2)=floor(tmp(2));
		// ROS_INFO("new_originIdx_offset: %d %d %d",new_originIdx_offset(0),new_originIdx_offset(1),new_originIdx_offset(2));

		// = ((new_originPos-mapCentrePos) / resolution_).array().floor().template cast<int>();

		// ROS_INFO("NEW origin pos: %f %f %f",new_originPos(0),new_originPos(1),new_originPos(2));
		// ROS_INFO("mapCentrePos: %f %f %f",mapCentrePos(0),mapCentrePos(1),mapCentrePos(2));
		offset=new_originIdx_offset;

		// mapCentrePos+=resolution_*new_originIdx_offset.cast<double>();  // map centre position is in global coordinate, but it is discrete
		
		
		Eigen::Vector3i tmp_2=((tmp_new_originPos-initPos) / resolution_).array().floor().template cast<int>();;
		// ROS_INFO("tmp_2: %d %d %d",tmp_2(0),tmp_2(1),tmp_2(2));


		mapCentrePos=initPos+resolution_*tmp_2.cast<double>();


		// ROS_INFO("mapCentrePos: %f %f %f",mapCentrePos(0),mapCentrePos(1),mapCentrePos(2));

		// ROS_INFO("offset: %d %d %d",offset(0),offset(1),offset(2));

		//set out of range grid to default value
		//consider offset x direction
		if(offset(0)>0)
		{
			for(int offset_x=0;offset_x<offset(0);offset_x++)
			{
				for(int j=0;j<_N;j++)
				{
					for(int i=0;i<_N;i++)
					{
						int tmp_index0=(offset_x+offset_sum(0))&_MASK;
						buffer_[tmp_index0+i*_N+j*_N*_N]=empty_element_;
					}
				}
			}
		}

		if(offset(0)<0)
		{
			for(int i=1;i<=fabs(offset(0));i++)
			{
				for(int k=0;k<_N;k++)
				{
					for(int j=0;j<_N;j++)
					{
						int tmp_index0=((_N-i+offset_sum(0))&_MASK);
						buffer_[tmp_index0+j*_N+k*_N*_N]=empty_element_;
					}
				}
			}
		}



		//consider offset y direction
		if(offset(1)>0)
		{
			for(int offset_y=0;offset_y<offset(1);offset_y++)
			{
				for(int j=0;j<_N;j++)
				{
					for(int i=0;i<_N;i++)
					{
						int tmp_index0=((offset_y+offset_sum(1))&_MASK);
						buffer_[i+tmp_index0*_N+j*_N*_N]=empty_element_;
					}
				}
			}
		}

		if(offset(1)<0)
		{
			for(int i=1;i<=fabs(offset(1));i++)
			{
				for(int k=0;k<_N;k++)
				{
					for(int j=0;j<_N;j++)
					{
						int tmp_index0=((_N-i+offset_sum(1))&_MASK);
						buffer_[j+tmp_index0*_N+k*_N*_N]=empty_element_;
					}
				}
			}
		}


		//consider offset z direction
		if(offset(2)>0)
		{
			for(int offset_z=0;offset_z<offset(2);offset_z++)
			{
				for(int j=0;j<_N;j++)
				{
					for(int i=0;i<_N;i++)
					{
						int tmp_index0=((offset_z+offset_sum(2))&_MASK);
						buffer_[i+j*_N+tmp_index0*_N*_N]=empty_element_;
					}
				}
			}
		}

		if(offset(2)<0)
		{
			for(int i=1;i<=fabs(offset(2));i++)
			{
				for(int k=0;k<_N;k++)
				{
					for(int j=0;j<_N;j++)
					{
						int tmp_index0=((_N-i+offset_sum(2))&_MASK);
						buffer_[j+k*_N+tmp_index0*_N*_N]=empty_element_;
					}
				}
			}
		}


		offset_sum+=offset;

		// ROS_INFO("out updateMapOrigin");




	}


};

}


#endif